#include "miau.h"
#include "SoR_Utils.h"
#include "legs.h"

#define STATE_IDLE 0
#define STATE_WALKING 1
#define STATE_WAITING_ORDERS 2


char command[50];
char byte;



int state=STATE_IDLE;

int state_count=0;
int sensors[10];
int pulse_counter=0;

void pulse() {
	pulse_counter++;
	if (pulse_counter >= 100) {
		rprintf("pulse\n");
		pulse_counter=0;
	}

}


void init() {


	//add 1.7s delay for potential power issues
	delay_cycles(65535);
	delay_cycles(65535);
	delay_cycles(65535);
	delay_cycles(65535);
	delay_cycles(65535);
	delay_cycles(65535);
	delay_cycles(65535);

	/****************INITIALIZATIONS*******************/
	//other stuff Im experimenting with for SoR
	uartInit();  // initialize the UART (serial port)
//    uartSetBaudRate(0, 38400); // set UARTE speed, for Bluetooth
    uartSetBaudRate(1, 115200); // set UARTD speed, for USB connection, up to 500k, try 115200 if it doesn't work
 //   uartSetBaudRate(2, 38400); // set UARTH speed
 //   uartSetBaudRate(3, 38400); // set UARTJ speed, for Blackfin
	//G=Ground, T=Tx (connect to external Rx), R=Rx (connect to external Tx)

	rprintfInit(uart1SendByte);// initialize rprintf system and configure uart1 (USB) for rprintf

	configure_ports(); // configure which ports are analog, digital, etc.


	// Initialize SPI
	//spiInit();
	spi_init();

	LED_on();

	rprintf("\r\nSystem Warmed Up");




	// initialize the timer system
 	init_timer0(TIMER_CLK_1024);
 	init_timer1(TIMER_CLK_64);
 	init_timer2(TIMER2_CLK_64);
 	init_timer3(TIMER_CLK_64);
 	init_timer4(TIMER_CLK_64);
 	init_timer5(TIMER_CLK_64);

	a2dInit(); // initialize analog to digital converter (ADC)
	a2dSetPrescaler(ADC_PRESCALE_DIV32); // configure ADC scaling
	a2dSetReference(ADC_REFERENCE_AVCC); // configure ADC reference voltage

//	wdt_enable(WDTO_8S);

	//let system stabelize for X time
	for(int i=0;i<16;i++)
		{
		a2dConvert8bit(i);//read each ADC once to get it working accurately
		delay_cycles(5000); //keep LED on long enough to see Axon reseting
		rprintf(".");
		}

	LED_off();


	// Set up compare mode interrupt
	OCR1A = 2500;  // It will trigger interrupt every 10ms
	timer_attach(TIMER1_COMPA_interrupt,pulse);

	sei();  // Enable global interrupts

	rprintf("Initialization Complete \r\n");

	/**************************************************/




	//reset all timers to zero
	reset_timer0();
	reset_timer1();
	reset_timer2();
	reset_timer3();
	reset_timer4();
	reset_timer5();

}




void do_cycle() {
	int i;

/*	for (i=0; i<10; i++) {
		sensors[i]=a2dConvert8bit(i);
		rprintf("%d ", sensors[i]);
	}
	rprintf("\n");*/

	if ((sensors[0] == 0) || (sensors[1] == 0)) {
		//center_all_legs();
		state = STATE_WAITING_ORDERS;
	}


	switch (state) {
		case STATE_IDLE:
			rprintf("IDLE \n");
			// Centers legs
			center_all_legs();

			state_count++;
			if (state_count > 1) state = STATE_WAITINT_ORDERS;
		break;
		case STATE_WALKING:
			//rprintf("%d\n", get_timer0_overflow());

			
			//walk();


		break;
		case STATE_WAITING_ORDERS:
			rprintf("WAITING ORDERS \n");

			// Read command from UART 1
			read_command(command);
			execute_command(command);
		
	}
	delay_ms(100);

//	wdt_reset();



}



